package driver.nxt;

import icommand.nxt.Motor;
import icommand.nxt.comm.NXTCommand;

import java.util.ArrayList;
import java.util.List;

import driver.Controller;
import driver.actions.Action;
import driver.events.Event;
import driver.exceptions.DestroyException;
import driver.exceptions.InitException;
import driver.nxt.action.NXTCenterTurningAction;
import driver.nxt.action.NXTCloseGrabberAction;
import driver.nxt.action.NXTMoveBackwardAction;
import driver.nxt.action.NXTMoveForwardAction;
import driver.nxt.action.NXTOpenGrabberAction;
import driver.nxt.action.NXTStopAction;
import driver.nxt.action.NXTTurnLeftAction;
import driver.nxt.action.NXTTurnLeftOnSpotAction;
import driver.nxt.action.NXTTurnRightAction;
import driver.nxt.action.NXTTurnRightOnSpotAction;
import driver.nxt.events.NXTBatteryEvent;
import driver.nxt.events.NXTEchoEvent;
import driver.nxt.events.NXTGrabberPositionEvent;
import driver.nxt.events.NXTLightEvent;
import driver.nxt.events.NXTSoundEvent;
import driver.nxt.events.NXTTouchEvent;
import driver.nxt.events.NXTTurnOffsetEvent;

/**
 * Implements the necessary Controller methods to communicate
 * with the Lego Mindstorms NXT Robot/Brick
 * 
 * @author David Martin
 */
public class NXTController extends Controller
{

  public static final int MOVE_TYPE_STOP               = 0;
  public static final int MOVE_TYPE_FORWARD            = 1;
  public static final int MOVE_TYPE_BACKWARD           = 2;
  public static final int MOVE_TYPE_TURN_LEFT_ON_SPOT  = 3;
  public static final int MOVE_TYPE_TURN_RIGHT_ON_SPOT = 4;
  // The starting speed of the NXT
  public static final int INITIAL_SPEED                = 270;
  // The maximum allowed speed of the NXT
  public static final int MAX_SPEED                    = 900;
  // The speed to turn the motors when turning on the spot
  public static final int TURN_ON_SPOT_SPEED           = 180;
  // The current speed of the robot
  protected int           currentSpeed;
  protected List<Event>   events;
  protected List<Action>  actions;
  protected Motor         grabber;
  protected Motor         left;
  protected Motor         right;
  protected int           moveType;

  public NXTController()
  {
    super();
    events = new ArrayList<Event>();
    actions = new ArrayList<Action>();
  }

  /**
   * Gets the corresponding events for each of the sensors on the
   * NXT. i.e. Ultrasonic, Light, Sound & Touch
   * 
   * @return a list of all NXT Events
   */
  public List<Event> getEvents()
  {
    if (events.size() == 0)
    {
      events.add(new NXTBatteryEvent());
      events.add(new NXTEchoEvent());
      events.add(new NXTLightEvent());
      events.add(new NXTSoundEvent());
      events.add(new NXTTouchEvent());
      events.add(new NXTGrabberPositionEvent());
      events.add(new NXTTurnOffsetEvent(this));
    }
    return events;
  }

  /**
   * Gets the actions for the NXT i.e. Moving, Open/Close Grabber
   * 
   * @return a list of all NXT actions
   */
  public List<Action> getActions()
  {
    if (actions.size() == 0)
    {
      actions.add(new NXTStopAction(this));
      actions.add(new NXTOpenGrabberAction(this));
      actions.add(new NXTCloseGrabberAction(this));
      actions.add(new NXTMoveForwardAction(this));
      actions.add(new NXTMoveBackwardAction(this));
      actions.add(new NXTTurnLeftAction(this));
      actions.add(new NXTTurnLeftOnSpotAction(this));
      actions.add(new NXTTurnRightAction(this));
      actions.add(new NXTTurnRightOnSpotAction(this));
      actions.add(new NXTCenterTurningAction(this));
    }
    return actions;
  }

  /**
   * Gets the NXT event based on the given safe event name
   * 
   * @param safeEventName the events safe name e.g.
   *          'sound_sensor'
   * @return the NXT Event
   */
  public Event getEventBySafeName(String safeEventName)
  {
    for (Event tempEvent : events)
    {
      if (tempEvent.getSafeEventName().equals(safeEventName))
      {
        return tempEvent;
      }
    }
    return null;
  }

  /**
   * Gets the NXT action based on the given safe action name
   * 
   * @param safeActionName the actions safe name e.g.
   *          'move_forward'
   * @return the NXT Action
   */
  public Action getActionBySafeName(String safeActionName)
  {
    for (Action tempAction : actions)
    {
      if (tempAction.getSafeActionName().equals(safeActionName))
      {
        return tempAction;
      }
    }
    return null;
  }

  /**
   * Establishes a communications channel over bluetooth with the
   * NXT
   * 
   * @throws driversuite.exceptions.InitException
   */
  public void init() throws InitException
  {
    try
    {
      setMoveType(NXTController.MOVE_TYPE_STOP);
      setGrabberMotor(Motor.A);
      setLeftMotor(Motor.C);
      setRightMotor(Motor.B);
      NXTCommand.open();
      setSpeed(NXTController.INITIAL_SPEED);
      getEvents();
      getActions();
    }
    catch (Exception e)
    {
      throw new InitException(e.getMessage());
    }
  }

  /**
   * Closes the bluetooth connection with the NXT
   * 
   * @throws driversuite.exceptions.DestroyException
   */
  public void destroy() throws DestroyException
  {
    try
    {
      NXTCommand.close();
    }
    catch (Exception e)
    {
      throw new DestroyException(e.getMessage());
    }
  }

  /**
   * Gets the current speed of the movement motors
   */
  public int getSpeed()
  {
    return currentSpeed;
  }

  /**
   * Set the speed of the movement motors
   * 
   * @params speed the speed to set
   */
  public void setSpeed(int speed)
  {
    currentSpeed = speed;
    left.setSpeed(currentSpeed);
    right.setSpeed(currentSpeed);
  }

  /**
   * Gets the difference in speed between the two movement
   * motors. The difference should be 0 when the steering is
   * centered, negative when turning left, and positive when
   * turning right
   * 
   * @return the turn offset
   */
  public int getTurnOffset()
  {
    return (left.getSpeed() - right.getSpeed()) / 2;
  }

  /**
   * Set the difference in power between the left and right
   * motor. If the difference is negative, the robot will steer
   * left. If the difference is positive the robot will turn
   * right. The value given will be added to one motor and
   * subtracted from the other, so the turn offset will be double
   * the set value.
   * 
   * @param turnOffset the turn offset value
   */
  public void setTurnOffset(int turnOffset)
  {
    left.setSpeed(left.getSpeed() + turnOffset);
    right.setSpeed(right.getSpeed() - turnOffset);
    switch (moveType)
    {
      case NXTController.MOVE_TYPE_FORWARD:
        left.forward();
        right.forward();
        break;
      case NXTController.MOVE_TYPE_BACKWARD:
        left.backward();
        right.backward();
        break;
      default:
        break;
    }
  }

  /**
   * Resets the difference between the left and right motors to
   * zero, thereby causing the robot to travel in a straight line
   */
  public void resetTurnOffset()
  {
    left.setSpeed(currentSpeed);
    right.setSpeed(currentSpeed);
    switch (moveType)
    {
      case NXTController.MOVE_TYPE_FORWARD:
        left.forward();
        right.forward();
        break;
      case NXTController.MOVE_TYPE_BACKWARD:
        left.backward();
        right.backward();
        break;
      default:
        break;
    }
  }

  public Motor getLeftMotor()
  {
    return left;
  }

  public void setLeftMotor(Motor motor)
  {
    this.left = motor;
  }

  public Motor getRightMotor()
  {
    return right;
  }

  public void setRightMotor(Motor motor)
  {
    this.right = motor;
  }

  public Motor getGrabberMotor()
  {
    return grabber;
  }

  public void setGrabberMotor(Motor motor)
  {
    this.grabber = motor;
  }

  public void setMoveType(int moveType)
  {
    this.moveType = moveType;
  }

  public void openGrabber()
  {
    grabber.setSpeed(100);
    grabber.forward();
    try
    {
      Thread.sleep(1000);
    }
    catch (InterruptedException e)
    {
      e.printStackTrace();
    }
    grabber.stop();
  }

  public void closeGrabber()
  {
    grabber.setSpeed(100);
    grabber.backward();
    try
    {
      Thread.sleep(1000);
    }
    catch (InterruptedException e)
    {
      e.printStackTrace();
    }
    grabber.stop();
  }

  public void turnLeftOnSpot()
  {
    setMoveType(NXTController.MOVE_TYPE_TURN_LEFT_ON_SPOT);
    go();
  }

  public void turnRightOnSpot()
  {
    setMoveType(NXTController.MOVE_TYPE_TURN_RIGHT_ON_SPOT);
    go();
  }

  public void forward()
  {
    setMoveType(NXTController.MOVE_TYPE_FORWARD);
    go();
  }

  public void backward()
  {
    setMoveType(NXTController.MOVE_TYPE_BACKWARD);
    go();
  }

  public void go()
  {
    switch (moveType)
    {
      case NXTController.MOVE_TYPE_FORWARD:
        left.forward();
        right.forward();
        break;
      case NXTController.MOVE_TYPE_BACKWARD:
        left.backward();
        right.backward();
        break;
      case NXTController.MOVE_TYPE_TURN_LEFT_ON_SPOT:
        int leftSpeedl = left.getSpeed();
        int rightSpeedl = right.getSpeed();
        left.setSpeed(NXTController.TURN_ON_SPOT_SPEED);
        right.setSpeed(NXTController.TURN_ON_SPOT_SPEED);
        left.backward();
        right.forward();
        left.setSpeed(leftSpeedl);
        right.setSpeed(rightSpeedl);
        break;
      case NXTController.MOVE_TYPE_TURN_RIGHT_ON_SPOT:
        int leftSpeedr = left.getSpeed();
        int rightSpeedr = right.getSpeed();
        left.setSpeed(NXTController.TURN_ON_SPOT_SPEED);
        right.setSpeed(NXTController.TURN_ON_SPOT_SPEED);
        left.forward();
        right.backward();
        left.setSpeed(leftSpeedr);
        right.setSpeed(rightSpeedr);
        break;
      default:
        break;
    }
  }

  public void stop()
  {
    moveType = NXTController.MOVE_TYPE_STOP;
    left.stop();
    right.stop();
    grabber.stop();
  }

  public boolean isMoving()
  {
    if (moveType == NXTController.MOVE_TYPE_STOP)
    {
      return false;
    }
    else
    {
      return true;
    }
  }
}
